import matplotlib.pyplot as plt
import math


# 绘制2D点
def plt_point2d(point, c='r', marker='o'):
    x, y = point
    return plt.scatter(x, y, c=c, marker=marker)


def plt_point2d_wave(point, c='g', marker='o'):
    xh, yh, wh = point
    if wh == 0:
        print("Error: w == 0")
    else:
        plt.scatter(xh/wh, yh/wh, c=c, marker=marker)
    # plt.show()


def plt_line(line, color='b'):
    a, b, c = line
    px = []
    py = []
    # ax + by + c = 0
    # x = -(c+by)/a
    # y = -(c+ax)/b
    if a == 0 and b == 0:
        return
    elif b != 0:
        px.append(-1)
        py.append(-(c - a) / b)
        px.append(1)
        py.append(-(c + a) / b)
    elif a != 0:
        px.append(-(c - b) / a)
        py.append(-1)
        px.append(-(c + b) / a)
        py.append(1)

    plt.plot(px, py, color)


def plt_line_theta(theta, d, color='y'):
    px = []
    py = []
    # cos(theta),sin(theta),d
    # x = - (d + sin(theta)y)/cos(theta)
    # y = - (d + cos(theta)x)/sin(theta)

    if math.cos(theta) != 0:
        px.append(-(d + math.sin(theta))/math.cos(theta))
        py.append(1)
        px.append(-(d - math.sin(theta))/math.cos(theta))
        py.append(-1)
    elif math.sin(theta) != 0:
        px.append(-1)
        py.append(-(d - math.cos(theta)) / math.sin(theta))
        px.append(1)
        py.append(-(d + math.cos(theta)) / math.sin(theta))

    plt.plot(px, py, color)


def find_cross(l1, l2):
    a1, b1, c1 = l1
    a2, b2, c2 = l2

    return b1 * c2 - b2 * c1, a2 * c1 - a1 * c2, a1 * b2 - a2 * b1


def make_line(p1, p2):
    x1, y1, w1 = p1
    x2, y2, w2 = p2
    return y1 * w2 - y2 * w1, x2 * w1 - x1 * w2, x1 * y2 - x2 * y1


if __name__ == '__main__':

    # (x, y)
    point2d = (.1, .2)
    plt_point2d(point2d)
    # (x, y, w)
    point2d_w1 = (.1, .2, .3)
    plt_point2d_wave(point2d_w1)
    point2d_w2 = (.2, .1, .3)
    plt_point2d_wave(point2d_w2)
    # l = (a, b, c) x.l = ax + by + c
    line1 = (.1, .3, .1)
    plt_line(line1)
    line2 = (.2, .1, .1)
    plt_line(line2)

    cross = find_cross(line1, line2)
    plt_point2d_wave(cross)

    line_w = make_line(point2d_w1, point2d_w2)
    plt_line(line_w)

    line3 = (.0, .3, .4)
    plt_line(line3)
    # l = (cos(theta), sin(theta),d)
    # line4 = (math.pi/4, .1)
    # plt_line_theta(line4[0], line4[1])
    # line5 = (-math.pi / 2, -.1)
    # plt_line_theta(line5[0], line5[1])
    line6 = (-math.pi/3, .2)
    plt_line_theta(line6[0], line6[1])

    # 设置坐标轴范围
    # plt.xlim((-1.2, 1.2))
    # plt.ylim((-1.2, 1.2))
    # 设置坐标轴名称
    plt.xlabel('X')
    plt.ylabel('Y')
    # 设置坐标轴刻度
    # my_x_ticks = np.arange(-1.2, 1.2, 0.2)
    # my_y_ticks = np.arange(-1.2, 1.2, 0.2)
    # plt.xticks(my_x_ticks)
    # plt.yticks(my_y_ticks)
    # 格子
    plt.grid(True)

    plt.show()

